#include "precom.h"
#include "Path5C.h"
#include <fstream>

using namespace std;
using namespace fmath;

Path5CPoint::Path5CPoint()
{
	m_point.set(0.f, 0.f, 0.f);
	m_eulerAngles.set(0.f, 0.f);
}

Path5C::Path5C()
{
}

Path5C::Path5C(wchar_t* a_fileName)
{
	LoadFromFile(a_fileName);
}

Path5C::~Path5C()
{
	ClearPath();
}

void Path5C::ClearPath()
{
	m_path.clear();
}

int Path5C::LoadFromFile(wchar_t* a_fileName)
{
	Path5CPoint knot;
	fstream file;
	file.open(a_fileName, ios::in);
	if( !file.is_open() )
		return CANT_OPEN_FILE;
	m_path.clear();
	while( !file.eof() )
	{
		file >> knot.m_point.x;
		file >> knot.m_point.y;
		file >> knot.m_point.z;
		file >> knot.m_eulerAngles.x;
		file >> knot.m_eulerAngles.y;
		m_path.push_back(knot);
	}
	file.close();

	return ResetSteps();
}

int Path5C::ResetSteps()
{
	if(m_path.empty()) 
		return NO_NEED_FOR_ACTION;

	m_next = m_path.begin();
	m_current = *m_next;
	m_prev = m_current;
	m_prevCurrent = m_current;
	++m_next;
	if(m_next == m_path.end())
		return NO_NEED_FOR_ACTION;

	return ALL_OK;
}

int Path5C::SaveToFile(wchar_t* a_fileName)
{
	list<Path5CPoint>::iterator iter = m_path.begin();
	fstream file;
	
	if( m_path.empty() ) return NO_NEED_FOR_ACTION;
	file.open(a_fileName, ios::out | ios::trunc);
	file.setf(ios_base::scientific, ios_base::floatfield);
	if( !file.is_open() )
		return CANT_OPEN_FILE;
	while( iter != m_path.end() )
	{
		file << iter->m_point.x;
		file << "  ";
		file << iter->m_point.y;
		file << "  ";
		file << iter->m_point.z;
		file << "  ";
		file << iter->m_eulerAngles.x;
		file << "  ";
		file << iter->m_eulerAngles.y;
		file << "  \n";
		++iter;
	}
	file.close();
	return ALL_OK;
}

/*************************************************
 *  Oblicza polozenie nastepnego punktu, zwraca 0,
 * modyfikuje argument a_distance, ktory jesli jest
 * dodatni wyznacza dlugosc kroku wzdloz nastepnego 
 * odcinka sciezki, zas jesli ujemny - ile zostalo
 * do konca bierzacego odcinak sciezki
 */
int Path5C::CalculateNextPoint(const Path5CPoint& a_curPos, float& a_distance)
{
	vector3 dir, dir2;
	vector2 ang;
	dir = m_next->m_point - a_curPos.m_point;
	ang = m_next->m_eulerAngles - a_curPos.m_eulerAngles;
	float dang = (a_distance / dir.length());
	dir.normalize();

	m_current = a_curPos;
	m_current.m_point += a_distance * dir;
	m_current.m_eulerAngles += dang * ang;
		
	//sprawdzenie czy nie wyszlismy poza odcinek
	dir2 = (m_next->m_point - m_prev.m_point);
	a_distance = -dir2.length();
	dir = (m_current.m_point - m_prev.m_point);
	a_distance += dir.length();

	return ALL_OK;
}

int Path5C::TakeStep(const Path5CPoint& a_curPos, float a_distance)
{
	if( m_next == m_path.end() )
		return NO_NEED_FOR_ACTION;
	float step = a_distance;

	CalculateNextPoint(a_curPos, a_distance);
	if(a_curPos == m_current 
		|| m_prevCurrent == m_current
		|| a_distance > EPS) {
	//while (a_distance > EPS) {
		m_prev = *m_next;
		++m_next;
		if( m_next == m_path.end() )
			return NO_NEED_FOR_ACTION;
		a_distance = step;
		CalculateNextPoint(a_curPos ,a_distance);
	}
	m_prevCurrent = m_current;

	return ALL_OK;
}

void Path5C::SkipPoint()
{
	if(m_path.empty()) return;
	m_prev = *m_next;
	++m_next;
}